#include "focus.h"
#include "motor_controller.h"
#include "sample_common.h"
#include "elog.h"
#include "elog_file.h"
#include "ws_constant.h"
#include "focus.pb.h"
#include "star_recognizer.h"
#include "ws_response.h"
#include "dwarf_utils.h"
#include "album_common.h"
#include "polyfit.h"
#include "timer_utils.h"
#include "iTips.h"

#include <functional>
#include <unordered_map>
#include <filesystem>
#include <cmath>
#include <vector>
#include <mutex>
#include <fstream>
#include <future>

#include "libhv/json.hpp"

#define TEST_ASTRO_AUTOFOCUS 0

namespace fs = std::filesystem;
using namespace std;

static StepMotorUser& motor_focus = MotorController::motorFocus();
static bool b_start_astro_autofocus = false;
static bool is_auto_focus = false;

// 长焦广角画面匹配
// static templateRect default_template_rect = {882, 497, 157, 86};
static templateRect default_template_rect = {588, 332, 104, 57};
static templateRect cur_template_rect = default_template_rect;
std::mutex mat_foreach_mtx;

std::vector<HfaData> hfa_data;
std::vector<std::vector<HfaData>> suitable_hfa_data;

bool if_first_star = true;

std::atomic<StateNotify> Focus::astro_fast_focus_state_ = STATE_IDLE;
std::atomic<StateNotify> Focus::astro_slow_focus_state_ = STATE_IDLE;
std::atomic<StateNotify> Focus::normal_focus_state_ = STATE_IDLE;
std::atomic<StateNotify> Focus::area_focus_state_ = STATE_IDLE;

ITips* Focus::itips_observer_ = nullptr;
bool Focus::b_try_astro_auto_focus_ = false;

static const std::string astro_config_file_path_ = "/userdata/cfg/astro_config.json";
std::mutex focus_state_mutex;
std::mutex focus_mutex;

Focus::Focus() {
    cam_tele_ = CamTele();
    cam_wide_ = CamWide();
}

Focus::~Focus() {

}

templateRect Focus::getCurTemplateRect() {
    return cur_template_rect;
}

//函数指针表
static unordered_map<int, int (Focus::*)(WsPacket& ws_packet)> focus_func_table {
    { CMD_FOCUS_AUTO_FOCUS, &Focus::autoFocus },
    { CMD_FOCUS_MANUAL_SINGLE_STEP_FOCUS, &Focus::manualSingleStepFocus },
    { CMD_FOCUS_START_MANUAL_CONTINU_FOCUS, &Focus::startManualContinuFocus },
    { CMD_FOCUS_STOP_MANUAL_CONTINU_FOCUS, &Focus::stopManualContinuFocus },
    { CMD_FOCUS_START_ASTRO_AUTO_FOCUS, &Focus::startAstroAutoFocusNew },
    { CMD_FOCUS_STOP_ASTRO_AUTO_FOCUS, &Focus::stopAstroAutoFocusNew },
};

int recognizeSunAndMoonByGrayImg(const Mat& img, Rect& target_bounding_box,Mat& countour_mask) {
    Mat img_gray = img.clone();

    Mat dstHist;
	float hranges[] ={0, 256};			// 取值范围[0,256)
	const float *ranges[] = {hranges};
	int size = 256;
	int channels = 0;

    double maxVal = 0;
	Point maxLoc;

    // //计算图像的直方图
	// calcHist(&img_gray, 1, &channels, Mat(), dstHist, 1, &size, ranges);
	// minMaxLoc(dstHist, NULL, &maxVal, NULL, &maxLoc);
	// double threshold = maxLoc.y;
    cv::Scalar mean;  
    cv::Scalar dev;
	cv::meanStdDev(img_gray, mean, dev);
	double threshold = (mean.val[0] + 3.0 * dev.val[0]) > 254 ? 254 : (unsigned int)(mean.val[0] + 3.0 * dev.val[0]);

    cv::threshold(img_gray, img_gray, threshold, 0, THRESH_TOZERO);
    // cv::threshold(img_gray, img_gray, threshold, 255, THRESH_BINARY);
    // imwrite("/test.jpg", img_gray);

	vector<vector<Point>> contours;
	findContours(img_gray, contours, noArray(), RETR_EXTERNAL, CHAIN_APPROX_SIMPLE);

	// Mat imgNosieContour = Mat::zeros(img_gray.rows, img_gray.cols, CV_8UC3);
	// drawContours(imgNosieContour, contours, -1, cv::Scalar(0,0,255));
    // imwrite("/userdata/moon/contour.png", imgNosieContour);

    cout << contours.size() << endl;
    double area_max = 0;
	for(int i = 0; i < contours.size(); i++) {
        double area = contourArea(contours[i]);
		if (area  > area_max) {
            area_max = area;
            target_bounding_box = boundingRect(contours[i]);
            countour_mask = Mat::zeros(img.size(),CV_8UC1);
            drawContours(countour_mask,contours,i,cv::Scalar(1),-1);
        }
	}
    
    return 0;
}

int Focus::handleMessage(WsPacket& ws_packet) {
    log_i("handle focus message, cmd = %d", ws_packet.cmd());

    //在表中查找函数指针并调用
    auto it = focus_func_table.find(ws_packet.cmd());
    if (it != focus_func_table.end()) {
        (this->*(it->second))(ws_packet);
    }
    else {
        log_i("Function not found");
    }

    return 0;
}

int Focus::autoFoucusClimbing(int direction, int time_out, int mode, int center_x, int center_y) {
    bool stop = false;
    unsigned long long pre_sharpness = 0;
    unsigned long long cur_sharpness = 0;
    unsigned long long max_sharpness = 0;
    int sharpness_up_count = 0;
    int sharpness_down_count = 0;
    int continue_count = 3;
    bool b_continue_up = false;
    bool b_continue_down = false;
    int count = 0;

    auto start_time = std::chrono::high_resolution_clock::now();

    if (0 == mode)
        getRoiaSharpness(pre_sharpness);
    else
        getWindowSharpness(center_x, center_y, pre_sharpness);
    log_i("pre sharpness = %lld", pre_sharpness);

    log_e("start auto focus climbing");

    // motor_focus.setParameterInPulse(2, 305, STEP_MOTOR_MINISTEP_1, STEP_MOTOR_RAMP_PULSE_0, direction);

    while (is_auto_focus) {
        count += 1;

        auto end_time = std::chrono::high_resolution_clock::now();
        if (std::chrono::duration_cast<std::chrono::seconds>(end_time -
                                                             start_time)
                .count() > time_out) {
            break;
        }
        
        motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, 2, 0);
        auto motor_start_time = std::chrono::high_resolution_clock::now();

        //等调焦电机停止，最多100ms
        while (STEP_MOTOR_STOPPED != motor_focus.driver.getMotionState()) {
            std::this_thread::sleep_for(std::chrono::milliseconds(1));
            auto motor_end_time = std::chrono::high_resolution_clock::now();
            if (std::chrono::duration_cast<std::chrono::milliseconds>(motor_end_time -
                                                                 motor_start_time)
                    .count() > 100) {
                break;
            }
        }

        if (0 == mode)
            getRoiaSharpness(cur_sharpness);
        else
            getWindowSharpness(center_x, center_y, cur_sharpness);

        // log_i("current sharpness = %lld, direction = %d", cur_sharpness,direction);

        if (cur_sharpness > pre_sharpness) {
            sharpness_up_count += 1;
            sharpness_down_count = 0;
        } else {
            sharpness_down_count += 1;
            sharpness_up_count = 0;
        }

        pre_sharpness = cur_sharpness;

        if(max_sharpness < cur_sharpness)
            max_sharpness = cur_sharpness;

        if (continue_count <= sharpness_up_count) {
            b_continue_up = true;
        }

        if (continue_count == sharpness_down_count) {
            b_continue_down = true;
            bool quit = false;
            if (b_continue_up) {
                direction = 1 - direction;
                log_i("set dirction test2, direction = %d", direction);
                usleep(100 * 1000);

                // motor_focus.setParameterInPulse(1, 305, STEP_MOTOR_MINISTEP_1, STEP_MOTOR_RAMP_PULSE_0, direction);
                for(int i = 0;i < continue_count * 2 + 20;i++) {
                    // motor_focus.run();
                    // motor_focus.waitMotorToStop();
                    motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, 1, 0);
                    auto motor_start_time = std::chrono::high_resolution_clock::now();

                    //等调焦电机停止，最多100ms
                    while (STEP_MOTOR_STOPPED != motor_focus.driver.getMotionState()) {
                        std::this_thread::sleep_for(std::chrono::milliseconds(1));
                        auto motor_end_time = std::chrono::high_resolution_clock::now();
                        if (std::chrono::duration_cast<std::chrono::milliseconds>(motor_end_time -
                                                                            motor_start_time)
                                .count() > 100) {
                            break;
                        }
                    }

                    usleep(10 * 1000);
                    
                    if (0 == mode)
                        getRoiaSharpness(cur_sharpness);
                    else
                        getWindowSharpness(center_x, center_y, cur_sharpness);

                    // log_i("current sharpness = %lld, direction = %d", cur_sharpness,direction);
                    
                    if(cur_sharpness >= max_sharpness) {
                        break;
                    } 

                    if (!is_auto_focus)
                        break;
                }

                break;        
            }
        }
    }

    return 0;
}

int Focus::autoExposure(){

    int ret = 0;

    if (cam_tele_.isCameraOpen() < 0) {
        ret = -CODE_CAMERA_TELE_CLOSED;
        return ret;
    }

    std::vector<double> exp_gear = {
        0.0001,
        0.000125,
        0.00015625,
        0.0002,
        0.00025,
        0.0003125,
        0.0004,
        0.0005,
        0.000625,
        0.0008,
        0.001,
        0.00125,
        0.0015625,
        0.002,
        0.0025,
        0.003125,
        0.004,
        0.005,
        0.00625,
        0.008,
        0.01,
        0.0125,
        0.0166666667,
        0.02,
        0.025,
        0.0333333333,
        0.04,
        0.05,
        0.0666666667,
        0.0769230769,
        0.1,
        0.125,
        0.1666666667,
        0.2,
        0.25,
        0.3333333333,
        0.4,
        0.5,
        // 0.6,
        // 0.8,
        // 1.0,
        // 1.3,
        // 1.6,
        // 2.0,
        // 2.5,
        // 3.2,
        // 4.0,
        // 5.0,
        // 6.0,
        // 8.0,
        // 10.0,
        // 13.0,
        // 15.0
    };

    double pre_exp,pre_hist_count_normalize;
    int adjust_nums = 0;
    // unsigned int hist[32];

    // std::string file_path = album_path + astro_path + getTimeStamp() + "/";
    // log_i("getAstroTestData file path is %s", file_path.c_str());
    // fs::create_directory(file_path);

    bool adjust_exp = true;
    double old_exp;
    while (adjust_exp)
    {
        double exp;

        ExposureSetting exp_setting;
        cam_tele_.getExp(exp_setting);

        exp = exp_setting.value;
        if(adjust_nums == 0){
            old_exp = exp;
        }
        log_i("exp is %f",exp);

        log_i("start get hist");
        void* nv12_buffer;
        MEDIA_BUFFER nv12_mediabuffer;
        nv12_mediabuffer = cam_tele_.getNewestNv12MediaBuffer();
        rkfree(nv12_mediabuffer);

        nv12_mediabuffer = cam_tele_.getNewestNv12MediaBuffer();
        if(!nv12_mediabuffer){
            log_i("get nv12_mediabuffer failed");
            ret = -CODE_PANORAMA_PHOTO_FAILED;
        }
        nv12_buffer = RK_MPI_MB_GetPtr(nv12_mediabuffer);
        // cv::Mat y_img;
        cv::Mat sun_moon_img,sun_moon_img_mask,hist_mat;
        cv::Mat nv12img,grayimg;
        // y_img = cv::Mat(720,1280,CV_8UC1,nv12_buffer);
        nv12img = cv::Mat(1080,1280,CV_8UC1,nv12_buffer);
        cvtColor(nv12img,grayimg,cv::COLOR_YUV2GRAY_NV12);
        Rect sun_moon_rect;
        Mat contours_mask;
        // recognizeSunAndMoonByGrayImg(y_img,sun_moon_rect);
        recognizeSunAndMoonByGrayImg(grayimg,sun_moon_rect,contours_mask);
        if(sun_moon_rect.width > 100 && sun_moon_rect.height > 100){
            // sun_moon_img = Mat(y_img,sun_moon_rect);
            sun_moon_img = Mat(grayimg,sun_moon_rect);
            sun_moon_img_mask = Mat(contours_mask,sun_moon_rect);
            // std::string img_path = file_path + std::to_string(adjust_nums) + ".png";
            // std::string img_sm_path = file_path + "sm_" + std::to_string(adjust_nums) + ".png";
            // log_i("img path = %s", img_path.c_str());
            // cout << "sun_moon_rect is " << sun_moon_rect << endl;
            // imwrite(img_path, y_img);
            // imwrite(img_sm_path, sun_moon_img);
            
            int channels[] = { 0 }; // 图像通道
            int histSize[] = { 256 }; // 直方图的大小
            float range[] = { 0, 256 }; // 像素值范围
            const float* ranges[] = { range };
            calcHist(&sun_moon_img,1,channels,sun_moon_img_mask,hist_mat,1,histSize,ranges);
        }else if(adjust_nums == 0){
            ret = -1;
        }else{
            ret = 1;
        }

        rkfree(nv12_mediabuffer);

        // ret = SAMPLE_COMM_ISP_sysctl_getHist(0,hist);
        log_i("end get hist");                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          
        if(ret == 0){
            //处理从isp获得的直方图
            //去除掉直方图中统计数量颜色最多的一个区块，也就是黑色区块
            float* hist = (float*)hist_mat.data;
            
            //以255的一半作为目标亮度，将0-255映射到-1到1之间进行归一化，目标亮度就可以记为0
            //求归一化后，整个直方图的所有像素之和，结果的绝对值越靠近0就代表曝光参数越合适，同时结果大于0表示需要调低曝光，小于0则代表需要提高曝光
            double hist_count_normalize = 0;
            for (int i = 0; i < 256; i++)
            {
                double light = (i - 127.5) / 128.0;
                hist_count_normalize += hist[i] * light;
            }
            
            if(adjust_nums != 0){
                //如果归一化的绝对值小于之前的参数，说明还可以继续优化曝光，如果大于则说明之前的优化结果已经是最佳
                if(abs(hist_count_normalize) < abs(pre_hist_count_normalize)){
                    adjust_exp = true;
                }else if(abs(hist_count_normalize) > abs(pre_hist_count_normalize)){
                    adjust_exp = false;
                }
            }
            log_i("hist_count_normalize is %f",hist_count_normalize);
            cout << "adjust_exp is " << adjust_exp << endl;

            //如果还需要继续调整曝光
            if(adjust_exp){
                if(hist_count_normalize > 0){
                    double next_exp = 0;
                    for (int j = 0; j < exp_gear.size() - 3; j += 2)
                    {
                        if(exp_gear[j] < exp && exp_gear[j + 2] >= exp){
                            next_exp = exp_gear[j];
                            break;
                        }
                    }
                    if(next_exp != 0){
                        cam_tele_.setExp(next_exp);
                        std::this_thread::sleep_for(std::chrono::milliseconds((int)(next_exp * 1000) + 100));
                    }
                }else if(hist_count_normalize < 0){
                    double next_exp = 0;
                    for (int j = 0; j < exp_gear.size() - 3; j += 2)
                    {
                        if(j - 2 >= 0 && exp_gear[j - 2] >= exp){
                            next_exp = exp_gear[j];
                            break;
                        }
                    }
                    if(next_exp != 0){
                        cam_tele_.setExp(next_exp);
                        std::this_thread::sleep_for(std::chrono::milliseconds((int)(next_exp * 1000) + 100));
                    }
                }
            }else{
                cam_tele_.setExp(pre_exp);
            }
            pre_hist_count_normalize = hist_count_normalize;
            pre_exp = exp;
        }else{
            break;
        }
        adjust_nums++;
    }
    return ret;
}

int Focus::checkSunMoonRect(){
    // unsigned int hist[32];
    // SAMPLE_COMM_ISP_sysctl_getHist(0,hist);
    // double hist_sum = 0;
    // int hist_count = 0;
    // for (int i = 0; i < 32; i++)
    // {
    //     hist_count += hist[i];
    //     hist_sum += i / 32.0 * 255 * hist[i];
    // }
    // double mean_birghtness2 = hist_sum / hist_count;
    // log_i("scence mean_birghtness2 is %f",mean_birghtness2);

    void* nv12_buffer;
    MEDIA_BUFFER nv12_mediabuffer;
    nv12_mediabuffer = cam_tele_.getNewestNv12MediaBuffer();
    rkfree(nv12_mediabuffer);

    nv12_mediabuffer = cam_tele_.getNewestNv12MediaBuffer();
    if(!nv12_mediabuffer){
        log_i("get nv12_mediabuffer failed");
    }
    nv12_buffer = RK_MPI_MB_GetPtr(nv12_mediabuffer);
    // cv::Mat y_img;
    cv::Mat nv12img,grayimg;
    // y_img = cv::Mat(720,1280,CV_8UC1,nv12_buffer);
    nv12img = cv::Mat(1080,1280,CV_8UC1,nv12_buffer);
    cvtColor(nv12img,grayimg,cv::COLOR_YUV2GRAY_NV12);
    rkfree(nv12_mediabuffer);
    // imwrite("/test.jpg",grayimg);
    double mean_birghtness2 = mean(grayimg)[0];
    Rect sun_moon_rect;
    Mat contours_mask;
    recognizeSunAndMoonByGrayImg(grayimg,sun_moon_rect,contours_mask);

    bool sun_moon_rect_ok = false;
    if(sun_moon_rect.width > 100 && sun_moon_rect.height > 100){
        sun_moon_rect_ok = true;
    }

    cout << "scence sun_moon_rect width is " << sun_moon_rect.width << " height is " << sun_moon_rect.height << endl;
    log_i("scence mean_brightness is %f",mean_birghtness2);
    if(sun_moon_rect_ok){
        return 1;
    }else{
        return 0;
    }
}

//通过广角相机拍摄检查是否是太阳月亮,0x0000001代表有太阳月亮，相反则是没有太阳月亮，0x0000010代表是明场，相反则是暗场
int Focus::checkWideSunMoon(){
    int old_wide_expmode,old_wide_gain;
    double old_wide_exp;
    cam_wide_.getExpMode(&old_wide_expmode);
    cam_wide_.getGain(&old_wide_gain);
    cam_wide_.getExp(&old_wide_exp);

    cam_wide_.setExpMode(1);
    cam_wide_.setGain(0);
    cam_wide_.setExp(0.03);

    std::this_thread::sleep_for(std::chrono::milliseconds(200));

    int ret = 1;
    double mean = 0;
    Mat wide_img;
    cam_wide_.getJpgDecodeMat(wide_img);
    if (!wide_img.empty()) {
        cvtColor(wide_img,wide_img,COLOR_BGR2GRAY);
        Rect pos_rect;
        Mat countour_mask;
        Mat countour_mask_not;
        recognizeSunAndMoonByGrayImg(wide_img,pos_rect,countour_mask);
        bitwise_not(countour_mask,countour_mask_not);
        mean = cv::mean(wide_img)[0];
        double area_mean = cv::mean(wide_img,countour_mask)[0];

        //计算区域外的平均标准差
        double area_not_mean,area_not_std;
        cv::Scalar area_not_mean_scalar,area_not_std_scalar;
        meanStdDev(wide_img,area_not_mean_scalar,area_not_std_scalar,countour_mask_not);
        area_not_mean = area_not_mean_scalar[0];
        area_not_std = area_not_std_scalar[0];

        //求梯度值
        Mat sobelx,sobely;
        Sobel(wide_img, sobelx, CV_64F, 1, 0, 5);
        Sobel(wide_img, sobely, CV_64F, 0, 1, 5);

        //因为梯度会和相邻区域比较，这里通过mask把梯度计算的不稳定因素尽量去除
        double uniformityScore = 0;
        if(!pos_rect.empty()){
            Mat sobel_mask,sobel_mask_not;
            Mat sobel_mask_kernel = getStructuringElement(MORPH_ELLIPSE,cv::Size(5, 5));
            dilate(countour_mask,sobel_mask,sobel_mask_kernel);
            bitwise_not(sobel_mask,sobel_mask_not);

            Mat magnitude;
            cv::sqrt(sobelx.mul(sobelx) + sobely.mul(sobely),magnitude);
            uniformityScore = cv::mean(magnitude,sobel_mask_not)[0];
        }

        log_i("wide mean brightness is %f",mean);
        log_i("wide countour mask mean brightness is %f",area_mean);
        log_i("wide countour mask not mean brightness is %f",area_not_mean);
        log_i("wide mean brightness rate is %f",area_mean / area_not_mean);
        log_i("wide countour mask not dev is %f",area_not_std);
        log_i("wide countour mask not uniformityScore is %f",uniformityScore);
        cout << "wide rect is " << pos_rect << endl;
        Point pos_rect_center_pt = Point(pos_rect.x + pos_rect.width / 2,pos_rect.y + pos_rect.height / 2);
        Rect center_rect = Rect(wide_img.cols / 15.0 * 7,wide_img.rows / 15.0 * 7,wide_img.cols / 15.0,wide_img.rows / 15.0);
        if(!(pos_rect & center_rect).empty() && (area_mean / area_not_mean > 8 || (uniformityScore > 0 && uniformityScore < 10))  && center_rect.contains(pos_rect_center_pt)){
            ret = 1;
        }else{
            ret = 0;
        }
    }

    if(ret){
        opMode_t old_expmode;
        double old_exp;
        int old_gain;
        cam_tele_.getExpMode(&old_expmode);
        cam_tele_.getExp(&old_exp);
        cam_tele_.getGain(&old_gain);

        cam_tele_.setExp(0.05);
        cam_tele_.setGain(0);

        std::this_thread::sleep_for(std::chrono::milliseconds(200));

        int sence_ret = checkSunMoonRect();

        if(!sence_ret){
            cam_tele_.setGain(old_gain);
            if(old_expmode == opMode_t::OP_AUTO){
                cam_tele_.setExpMode(old_expmode);
            }else{
                cam_tele_.setExp(old_exp);
            }
            std::this_thread::sleep_for(std::chrono::milliseconds(int(old_exp * 1000) + 200));
        }

        ret = sence_ret;
    }

    cam_wide_.setGain(old_wide_gain);
    if(old_wide_expmode == 3){
        cam_wide_.setExpMode(old_wide_expmode);
    }else{
        cam_wide_.setExp(old_wide_exp);
    }

    if (!wide_img.empty()) {
        if(mean < 20){
            return ret;
        }else{
            return ret | 0x0000010;
        }
    } else {
        return ret | 0x0000010;// 广角没打开的话，默认明场，出错成本较低
    }
}



int Focus::autoFocus(int mode, int center_x, int center_y) {
    bool stop = false;
    unsigned long long pre_sharpness = 0;
    unsigned long long cur_sharpness = 0;
    unsigned long long max_sharpness = 0;
    int direction = 0;
    int sharpness_up_count = 0;
    int sharpness_down_count = 0;
    int continue_count = 4;
    bool b_continue_up = false;    //是否出现过连续2次增加的情况
    bool b_continue_down = false;  //是否出现过连续1次减少的情况
    int count = 0;
    int total_count = 0;
    int time_out = 3;
    int max_pos = 0;
    int cur_pos = 0;
    int stop_pos = 0;
    float current_sharpness_ratio = 0.0;
    float sharpness_down_ratio_thres = 0.98;
    double exp = 0.0;
    int motor_continue_speed = 300;

    // if (is_auto_focus) {
    //     return 0;
    // }

    log_i("start auto focus");
    log_i("reset motor, direction = %d", direction);

    int start_position;
    motor_focus.resetInPulse(direction, start_position);

    direction = 1 - direction;

    cam_tele_.getExp(&exp);
    if (exp <= 0.033)
        motor_continue_speed = 300 * 32;    // 使用32细分，提高脉冲频率，提高单片机的响应速度
    else
        motor_continue_speed =   10 / exp * 32;
    
    log_i("set motor speed = %d", motor_continue_speed);
    time_out = 300.0 / motor_continue_speed * 32 + 2;
    if (time_out > 32)
        time_out = 32;
    log_i("set time_out = %d", time_out);

    motor_focus.driver.run(1, STEP_MOTOR_MINISTEP_32, direction, motor_continue_speed, 2, 0);
    
    auto start_time = std::chrono::high_resolution_clock::now();
    
    is_auto_focus = true;
    while(is_auto_focus) {
        // is_auto_focus = true;
        count += 1;

        auto end_time = std::chrono::high_resolution_clock::now();
        if (std::chrono::duration_cast<std::chrono::seconds>(end_time -
                                                             start_time)
                .count() > time_out) {
            stop = true;
            is_auto_focus = false;
            motor_focus.stop();
            usleep(20 * 1000);
            continue;
        }
        
        int relative_position;
        motor_focus.getRelativePosition(relative_position);
        cur_pos = relative_position == 0 ? cur_pos : relative_position;   // 碰到限位后电机会停，相对位置清零，要保留碰到限位前最后的位置
        // log_i("before get sharpness, position = %d\t", cur_pos);

        if (0 == mode)
            getRoiaSharpness(cur_sharpness);
        else
            getWindowSharpness(center_x, center_y, cur_sharpness);

        //  log_i("current sharpness = %lld, direction = %d\t", cur_sharpness,direction);

        motor_focus.getRelativePosition(relative_position);
        cur_pos = relative_position == 0 ? cur_pos : relative_position;   // 碰到限位后电机会停，相对位置清零，要保留碰到限位前最后的位置

        if (count <= 5)
            continue;
        
        if (count == 6) {
            if (0 == mode)
                getRoiaSharpness(pre_sharpness);
            else
                getWindowSharpness(center_x, center_y, pre_sharpness);
            continue;
        }
        
        current_sharpness_ratio = 1.0 * cur_sharpness / pre_sharpness;
        if (cur_sharpness > pre_sharpness) {
            sharpness_up_count += 1;
            sharpness_down_count = 0;
        } else {
            if (current_sharpness_ratio < sharpness_down_ratio_thres) {
                sharpness_down_count += 1;
                sharpness_up_count = 0;
            }
        }

        pre_sharpness = cur_sharpness;

        if (2 <= sharpness_up_count) {
            b_continue_up = true;
        }

        if(max_sharpness < cur_sharpness) {
            max_sharpness = cur_sharpness;
            max_pos = cur_pos;
        }
        
        if (1  == sharpness_down_count) {
            if (b_continue_up) {
                direction = 1 - direction;
                stop_pos = cur_pos;
                log_i("after stop motor, position = %d", stop_pos);
                motor_focus.stop();
                usleep(20 * 1000);
                break;
            }
        } 

        // 检测是否到近焦限位，全程没有满足要求的连续上升下降，但可以回到最大值再细调
        if(motor_focus.getLimitState(FOCUS_NEAR) == STEP_MOTOR_LIMITED) {
            log_i("Near foucs limit");
            direction = 1 - direction;
            stop_pos = cur_pos;
            log_i("after stop motor, position = %d", stop_pos);
            break;
        }
    }
    
    motor_focus.stop();
    usleep(20 * 1000);

    if (stop_pos - max_pos <= 0) {
        // is_auto_focus = false;
        return 0;
    }

    log_i("search back1 position = %d, stop_pos = %d, max_pos = %d", max_pos, stop_pos, max_pos);
    // 脉冲模式也用32细分，记步可以直接用，不用/32

    motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_32, direction, 305 * 32, stop_pos - max_pos, 0);
    auto motor_start_time = std::chrono::high_resolution_clock::now();

    //等调焦电机停止，最多1s
    while (STEP_MOTOR_STOPPED != motor_focus.driver.getMotionState()) {
        std::this_thread::sleep_for(std::chrono::milliseconds(1));
        auto motor_end_time = std::chrono::high_resolution_clock::now();
        if (std::chrono::duration_cast<std::chrono::milliseconds>(motor_end_time -
                                                                motor_start_time)
                .count() > 1000) {
            break;
        }
    }

    //爬坡算法
    autoFoucusClimbing(direction, time_out, mode, center_x, center_y);

    motor_focus.stop();
    usleep(20 * 1000);
    // is_auto_focus = false;

    return 0;
}
    
int Focus::manualSingleStepFocus(MotorFocusDirection direction) {
    int ret;

    ret = motor_focus.setParameterInPulse(1, 305, STEP_MOTOR_MINISTEP_1, STEP_MOTOR_RAMP_PULSE_0, direction);
    if (ret < 0) {
        return ret;
    }
    motor_focus.run();

    motor_focus.waitMotorToStop();

    return 0;
}

int Focus::startManualContinuFocus(MotorFocusDirection direction) {
    int ret;
    ret = motor_focus.setParameterInPulse(400, STEP_MOTOR_MINISTEP_8, STEP_MOTOR_RAMP_PULSE_0, direction);
    
    motor_focus.run();
    
    return ret;
}
    
int Focus::stopManualContinuFocus() {
    motor_focus.stop();

    return 0;
}

//主窗口清晰度
int Focus::getRoiaSharpness(unsigned long long& value) {
    rk_aiq_isp_stats_t* stats_ref = NULL;
    int ret;
    if (ret = cam_tele_.isCameraOpen() < 0) {
        return ret;
    }
    value = SAMPLE_COMM_ISP_sysctl_getSharpness(0, stats_ref);
    return 0;
}

//对焦区域窗口清晰度
int Focus::getWindowSharpness(int center_x, int center_y, unsigned long long& value) {
    rk_aiq_isp_stats_t* stats_ref = NULL;
    int ret;
    if (ret = cam_tele_.isCameraOpen() < 0) {
        return ret;
    }
    
    value = SAMPLE_COMM_ISP_sysctl_getWindowSharpness(0, center_x, center_y, stats_ref);
    return 0;
}

std::tuple<cv::Rect, double> Focus::matchTemplate() {
    cv::Mat large_image;
    MEDIA_BUFFER dst_mb = cam_tele_.getRgbMB_720p(1280, 720);
    cv::Mat small_image(720, 1280, CV_8UC3, (unsigned char*)RK_MPI_MB_GetPtr(dst_mb));
    // cv::Mat small_image_ori = small_image.clone();
    cam_wide_.getJpgDecodeMat(large_image);
    if (large_image.empty()) {
        cv::Rect match_rect(-1, -1, -1, -1);
        return std::make_tuple(match_rect, 0);
    }

    // cv::resize(small_image, small_image, cv::Size(128, 72));
    cv::resize(small_image, small_image, cv::Size(default_template_rect.width, default_template_rect.height));

    // 计算裁剪区域的坐标
    int height = large_image.rows;
    int width = large_image.cols;
    int crop_height = height / 6;
    int crop_width = width / 6;
    int crop_start_x = (width - crop_width) / 2;
    int crop_end_x = crop_start_x + crop_width;
    int crop_start_y = (height - crop_height) / 2;
    int crop_end_y = crop_start_y + crop_height;
    cv::Mat large_image1 = large_image(cv::Range(crop_start_y, crop_end_y), cv::Range(crop_start_x, crop_end_x));

    // 记录开始时间
    double start_time = cv::getTickCount();

    // 执行模板匹配
    cv::Mat result;
    cv::matchTemplate(large_image1, small_image, result, cv::TM_CCOEFF_NORMED);
    double min_val, max_val;
    cv::Point min_loc, max_loc;
    cv::minMaxLoc(result, &min_val, &max_val, &min_loc, &max_loc);

    // 获取匹配位置
    cv::Point top_left(max_loc.x + crop_start_x, max_loc.y + crop_start_y);
    cv::Point bottom_right(top_left.x + small_image.cols, top_left.y + small_image.rows);

    // 在大图上绘制矩形框标记匹配位置
    cv::Mat matched_image = large_image.clone();
    cv::rectangle(matched_image, top_left, bottom_right, cv::Scalar(0, 255, 0), 2);

    // 计算并打印执行时间
    double end_time = cv::getTickCount();
    double execution_time = (end_time - start_time) / cv::getTickFrequency();
    log_i("cost time = %f", execution_time);

    rkfree(dst_mb);

    cv::Rect match_rect(top_left, bottom_right);
    return std::make_tuple(match_rect, max_val);
}

int Focus::autoFocus(WsPacket& ws_packet) {
    if (!focus_mutex.try_lock()) {
        log_e("INVALID CMD: motor busy!");
        // wsCommonResponse(CMD_FOCUS_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);
        // return -1;
        is_auto_focus = false;
        while (normal_focus_state_ != STATE_IDLE || area_focus_state_ != STATE_IDLE) {
            usleep(50 * 1000);
        }
    } else {
        focus_mutex.unlock();
    }

    std::lock_guard<std::mutex> guard(focus_mutex);

    int ret;
    ReqNormalAutoFocus req_normal_autofocus;
    string serialized_data = ws_packet.data();

    if (!req_normal_autofocus.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    int mode = req_normal_autofocus.mode();
    int center_x = req_normal_autofocus.center_x();
    int center_y = req_normal_autofocus.center_y();
    log_i("mode = %d, center_x = %d, center_y = %d", mode, center_x, center_y);

    if (cam_tele_.isCameraOpen() < 0) {
        wsCommonResponse(CMD_FOCUS_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, -CODE_CAMERA_TELE_CLOSED);
        return -CODE_CAMERA_TELE_CLOSED;
    }

    if (mode == 0)
        setAndNotifyFocusState(STATE_RUNNING, CAM_FUNCTION_NORMAL_AUTO_FOCUS);
    else
        setAndNotifyFocusState(STATE_RUNNING, CAM_FUNCTION_AREA_AUTO_FOCUS);

    // opMode_t expmode;
    // double oldexp;
    // int oldgain;
    // cam_tele_.getExpMode(&expmode);
    // cam_tele_.getExp(&oldexp);
    // cam_tele_.getGain(&oldgain);

    int has_sun_moon = checkWideSunMoon();

    //如果有太阳月亮
    if((has_sun_moon & 0x0000001) == 0x0000001){
        autoExposure();
    }

    ret = autoFocus(mode, center_x, center_y);

    // if(has_sun_moon && expmode == opMode_t::OP_MANUAL){
    //     cam_tele_.setExp(oldexp);
    //     cam_tele_.setGain(oldgain);
    // }

    if (!is_auto_focus) {
        wsCommonResponse(CMD_FOCUS_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, ret);
    
        if (mode == 0) {
            setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_NORMAL_AUTO_FOCUS);
            setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_NORMAL_AUTO_FOCUS);
        } else {
            setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_AREA_AUTO_FOCUS);
            setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_AREA_AUTO_FOCUS);
        }

        return 0;
    }

    cv::Rect match_rect;
    double match_value;
    if (ret == 0 && mode == 0) {
        std::tie(match_rect, match_value) = matchTemplate();
        log_i("matched x = %d, y = %d, width = %d, height = %d, value = %f", match_rect.x, match_rect.y, match_rect.width, match_rect.height, match_value);
        if (match_rect.x < 0) {}
        else {
            double diff_ratio_w = fabs((match_rect.x - default_template_rect.left_top_w) * 1.0 / default_template_rect.left_top_w) * 100;
            double diff_ratio_h = fabs((match_rect.y - default_template_rect.left_top_h) * 1.0 / default_template_rect.left_top_h) * 100;
            log_i("diff_ratio_w = %.f, diff_ratio_h = %.f", diff_ratio_w, diff_ratio_h);
            if (diff_ratio_w < 10.0 && diff_ratio_h < 10) {
                cur_template_rect.left_top_w = match_rect.x;
                cur_template_rect.left_top_h = match_rect.y;
                cur_template_rect.width = match_rect.width;
                cur_template_rect.height = match_rect.height;
                log_i("cur_template_rect left_top_w = %d, left_top_h = %d", match_rect.x, match_rect.y);
                wsResNotifyPictureMatching(CMD_NOTIFY_TELE_WIDI_PICTURE_MATCHING, MODULE_FOCUS, CMD_TYPE_NOTIFICATION, ret, match_rect.x, match_rect.y, match_rect.width, match_rect.height, match_value);
            }
        }
    }

    wsCommonResponse(CMD_FOCUS_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, ret);
    
    if (mode == 0) {
        setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_NORMAL_AUTO_FOCUS);
        setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_NORMAL_AUTO_FOCUS);
    } else {
        setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_AREA_AUTO_FOCUS);
        setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_AREA_AUTO_FOCUS);
    }

    return 0;
}

templateRect Focus::calcCurTemplateRect() {
    cv::Rect match_rect;
    double match_value;

    std::tie(match_rect, match_value) = matchTemplate();
    log_i("calcCurTemplateRect matched x = %d, y = %d, width = %d, height = %d, value = %f",
           match_rect.x, match_rect.y, match_rect.width, match_rect.height, match_value);
    if (match_rect.x >= 0) {
        double diff_ratio_w = fabs((match_rect.x - default_template_rect.left_top_w) * 1.0 /
                                default_template_rect.left_top_w) * 100;
        double diff_ratio_h = fabs((match_rect.y - default_template_rect.left_top_h) * 1.0 /
                                default_template_rect.left_top_h) * 100;
        log_i("diff_ratio_w = %.f, diff_ratio_h = %.f", diff_ratio_w, diff_ratio_h);
        if (diff_ratio_w < 10.0 && diff_ratio_h < 10) {
            cur_template_rect.left_top_w = match_rect.x;
            cur_template_rect.left_top_h = match_rect.y;
            cur_template_rect.width = match_rect.width;
            cur_template_rect.height = match_rect.height;

            log_i("cur_template_rect left_top_w = %d, left_top_h = %d", match_rect.x, match_rect.y);
            wsResNotifyPictureMatching(CMD_NOTIFY_TELE_WIDI_PICTURE_MATCHING, MODULE_FOCUS, CMD_TYPE_NOTIFICATION, 0, match_rect.x, match_rect.y, match_rect.width, match_rect.height, match_value);

            return cur_template_rect;
        }
    }

    return default_template_rect;
}

int Focus::manualSingleStepFocus(WsPacket& ws_packet) {
    if (!focus_mutex.try_lock()) {
        log_e("INVALID CMD: motor busy!");
        wsCommonResponse(CMD_FOCUS_MANUAL_SINGLE_STEP_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);
        return -1;
    }

    std::lock_guard<std::mutex> guard(focus_mutex, std::adopt_lock);

    log_i("manualSingleStepFocus");
    ReqManualSingleStepFocus req_manual_single_step_focus;
    string serialized_data = ws_packet.data();
    if (!req_manual_single_step_focus.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    MotorFocusDirection direction = req_manual_single_step_focus.direction() == 0 ? FOCUS_FAR : FOCUS_NEAR;
    int ret = manualSingleStepFocus(direction);
    if (ret < 0)
        wsCommonResponse(CMD_FOCUS_MANUAL_SINGLE_STEP_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, -WS_INVAID_PARAM);
    else
        wsCommonResponse(CMD_FOCUS_MANUAL_SINGLE_STEP_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);

    return 0;
}

int Focus::startManualContinuFocus(WsPacket& ws_packet) {
    if (!focus_mutex.try_lock()) {
        log_e("INVALID CMD: motor busy!");
        wsCommonResponse(CMD_FOCUS_START_MANUAL_CONTINU_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);
        return -1;
    }

    std::lock_guard<std::mutex> guard(focus_mutex, std::adopt_lock);

    ReqManualContinuFocus req_manual_continu_focus;
    string serialized_data = ws_packet.data();
    if (!req_manual_continu_focus.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    MotorFocusDirection direction = req_manual_continu_focus.direction() == 0 ? FOCUS_FAR : FOCUS_NEAR;
    int ret = startManualContinuFocus(direction);
    if (ret < 0)
        wsCommonResponse(CMD_FOCUS_START_MANUAL_CONTINU_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, -WS_INVAID_PARAM);
    else
        wsCommonResponse(CMD_FOCUS_START_MANUAL_CONTINU_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);

    return 0;
}

int Focus::stopManualContinuFocus(WsPacket& ws_packet) {
    std::lock_guard<std::mutex> guard(focus_mutex);

    stopManualContinuFocus();
    wsCommonResponse(CMD_FOCUS_STOP_MANUAL_CONTINU_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);

    return 0;
}

int Focus::getHfd(const cv::Mat& img_whole, float& hfa_equivalent_radiu) {
    float v_weight = 3;  // weight for calculate hfd (\sigma V^weight*d)/(\sigma V^weight)
    int integrated_circle_radius = 70;
    int star_area_thresh = 50;
    int search_radius = integrated_circle_radius;
    int bound_thresh = integrated_circle_radius; // shoun be large than Intergrated_circle_Radius. Use to judge if a conneced region near to the img bound

    static cv::Point2f last_star_centroid;

    int sub_img_width_height = 2 * (integrated_circle_radius + 50);
    cv::Point2f sub_img_center(sub_img_width_height / 2, sub_img_width_height / 2);

    // Step 1: read img, convert into HSV, and extract brightness channel
    cv::Mat img_src = img_whole;

    int img_width = img_src.cols;
    int img_height = img_src.rows;

    cv::Mat img_crop;
    if (!if_first_star) {
        cv::Rect crop(last_star_centroid.x - sub_img_width_height / 2, last_star_centroid.y - sub_img_width_height / 2, sub_img_width_height, sub_img_width_height);
        cv::Rect img_rect(0, 0, img_width, img_height);
        img_crop = img_src(img_rect & crop);

        // 求小图中上次质心坐标，小图不一定为正方形，质心坐标也不一定在小图中心
        if (crop.x < 0)
            sub_img_center.x = img_crop.cols - sub_img_width_height / 2;
        if (crop.y < 0)
            sub_img_center.y = img_crop.rows - sub_img_width_height / 2;
    } else {
        img_crop = img_src;
    }

    cv::Mat img_hsv;    
    cv::cvtColor(img_crop, img_hsv, cv::COLOR_RGB2HSV);

    // 使用亮度通道
    cv::Mat img_brightness;
    cv::extractChannel(img_hsv, img_brightness, 2);

    // Step 2: noise_reduction
    cv::medianBlur(img_brightness, img_brightness, 3);
    cv::medianBlur(img_brightness, img_brightness, 3);
    cv::GaussianBlur(img_brightness, img_brightness, cv::Size(5, 5), 0);

    // Step 3: threshold
    cv::Mat img_threshold;
    cv::threshold(img_brightness, img_threshold, 0, 255, cv::THRESH_TRIANGLE);

    cv::Mat img_threshold_norm = img_threshold / 255;
    double sum_th = cv::sum(img_threshold_norm)[0];

    int sub_img_width = img_threshold.cols;
    int sub_img_height = img_threshold.rows;
    double abnormal_thresharea = sub_img_width * sub_img_height / 2;// 判断阈值分割后星点大小是否有问题的阈值

    if (sum_th > abnormal_thresharea) {
        // 继续下一个图像处理
        std::cout << "star is too big" << std::endl;

        if (!if_first_star) {// 全局搜索星点
            if_first_star = true;

            suitable_hfa_data.push_back(hfa_data);
            hfa_data.clear();
        }
        
        return -1;
    }

    // Step 4: morphology operations
    cv::Mat kernel = cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(5, 5));
    cv::morphologyEx(img_threshold, img_threshold, cv::MORPH_OPEN, kernel);// 开运算，除噪点
    cv::morphologyEx(img_threshold, img_threshold, cv::MORPH_CLOSE, kernel, cv::Point(-1, -1), 5);// 闭运算，闭合星点

    // Step 5: choose a suitable star
    cv::Mat labels, stats, centroids;
    int num_labels = cv::connectedComponentsWithStats(img_threshold, labels, stats, centroids, 8);

    int suitable_param = -1;
    int suitable_star_label = -1;
    for (int label = 1; label < num_labels; ++label) {
        if (if_first_star) {
            double centroid_x = centroids.at<double>(label, 0);
            double centroid_y = centroids.at<double>(label, 1);

            if (centroid_x < integrated_circle_radius || img_width - centroid_x < integrated_circle_radius || 
                centroid_y < integrated_circle_radius || img_height - centroid_y < integrated_circle_radius) {// 星点不能在图像边界
                continue;
            }

            int area = stats.at<int>(label, cv::CC_STAT_AREA);// 星点要够大
            if (area < star_area_thresh) {
                continue;
            }

            if (area > suitable_param) {// 找最亮的星星
                suitable_param = area;
                suitable_star_label = label;
            }
        } else {
            double sub_img_star_centroid_x = centroids.at<double>(label, 0);
            double sub_img_star_centroid_y = centroids.at<double>(label, 1);

            double distance = cv::norm(sub_img_center - cv::Point2f(sub_img_star_centroid_x, sub_img_star_centroid_y));
            if (distance > integrated_circle_radius) {// 在第一颗星附近搜索
                continue;
            }

            if (suitable_param < 0) {
                suitable_param = distance;
                suitable_star_label = label;
            } else {
                if (distance < suitable_param) {// 找距离第一颗星最近的星星
                    suitable_param = distance;
                    suitable_star_label = label;
                }
            }
        }
    }

    if (suitable_star_label < 0) {// 没有合适的星点
        if (!if_first_star) {// 全局搜索星点
            if_first_star = true;

            suitable_hfa_data.push_back(hfa_data);
            hfa_data.clear();
        }

        std::cout << "no suitable star" << std::endl;
        return -1;
    } else {
        if (if_first_star) {
            if_first_star = false;

            last_star_centroid.x = centroids.at<double>(suitable_star_label, 0);
            last_star_centroid.y = centroids.at<double>(suitable_star_label, 1);
        } else {
            double centroid_x = centroids.at<double>(suitable_star_label, 0) - sub_img_center.x + last_star_centroid.x;
            double centroid_y = centroids.at<double>(suitable_star_label, 1) - sub_img_center.y + last_star_centroid.y;

            if (centroid_x < integrated_circle_radius || img_width - centroid_x < integrated_circle_radius || 
                centroid_y < integrated_circle_radius || img_height - centroid_y < integrated_circle_radius) {// 星点不能在图像边界
                
                if_first_star = true;

                suitable_hfa_data.push_back(hfa_data);
                hfa_data.clear();

                std::cout << "star move to border" << std::endl;
                return -2;
            } else {
                last_star_centroid.x = centroid_x;
                last_star_centroid.y = centroid_y;
            }
        }
    }

    // Step 6: Cog Centroid method
    cv::Mat star_mask = cv::Mat::zeros(sub_img_height, sub_img_width, CV_8UC1);
    cv::Rect star_rect(stats.at<int>(suitable_star_label, cv::CC_STAT_LEFT), stats.at<int>(suitable_star_label, cv::CC_STAT_TOP), stats.at<int>(suitable_star_label, cv::CC_STAT_WIDTH), stats.at<int>(suitable_star_label, cv::CC_STAT_HEIGHT));
    star_mask(star_rect) = 1;

    cv::Mat star_img = img_brightness.mul(star_mask);

    cv::Moments moments = cv::moments(star_img);
    float star_img_sum = moments.m00;
    float star_img_x_cog_sum = moments.m10;
    float star_img_y_cog_sum = moments.m01;

    float star_img_x_cog = star_img_x_cog_sum / star_img_sum;
    float star_img_y_cog = star_img_y_cog_sum / star_img_sum;

    cv::Point2f star_img_cog(star_img_x_cog, star_img_y_cog);

    // std::cout << "cog centroid_x " << star_img_x_cog << std::endl;
    // std::cout << "cog centroid_y " << star_img_y_cog << std::endl;

    // Step 7: sustract the background
    float calculate_img_mean = 0.0;
    if (if_first_star) {
        cv::Rect crop(last_star_centroid.x - sub_img_width_height / 2, last_star_centroid.y - sub_img_width_height / 2, sub_img_width_height, sub_img_width_height);
        cv::Rect img_rect(0, 0, img_width, img_height);
        cv::Mat img_brightness_crop = img_brightness(img_rect & crop);
        calculate_img_mean = cv::mean(img_brightness_crop)[0];
    }
    calculate_img_mean = cv::mean(img_brightness)[0];

    cv::Mat img_substract_bg = img_brightness - calculate_img_mean;
    cv::threshold(img_substract_bg, img_substract_bg, 0, 0, cv::THRESH_TOZERO);

    // Step 8: hfa
    cv::medianBlur(img_substract_bg, img_substract_bg, 5);

    cv::Mat calculate_mask(sub_img_height, sub_img_width, CV_8UC1, cv::Scalar(0));
    cv::circle(calculate_mask, star_img_cog, integrated_circle_radius, cv::Scalar(1), -1);// 设置计算的圆邻域mask

    img_substract_bg = img_substract_bg.mul(calculate_mask);

    img_substract_bg.convertTo(img_substract_bg, CV_32F);
    cv::pow(img_substract_bg, 0.5, img_substract_bg);

    double sub_pixel_max;
    cv::minMaxLoc(img_substract_bg, nullptr, &sub_pixel_max);

    float img_substract_bg_sum = cv::sum(img_substract_bg)[0];
    float img_substract_bg_count = cv::countNonZero(calculate_mask);
    float img_substract_bg_mean = img_substract_bg_sum / img_substract_bg_count;

    float flux_thresh = 0.5 * (sub_pixel_max - img_substract_bg_mean);

    cv::threshold(img_substract_bg, img_substract_bg, flux_thresh, 1, cv::THRESH_BINARY);

    float hfa = cv::countNonZero(img_substract_bg);

    hfa_equivalent_radiu = std::sqrt(hfa / M_PI);

    // std::cout << "hfa_equivalent_radiu " << hfa_equivalent_radiu << std::endl;

    return 0;
}

int Focus::startAstroAutoFocusNew(WsPacket& ws_packet) {
    if (!focus_mutex.try_lock()) {
        log_e("INVALID CMD: motor busy!");
        wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);
        return -1;
    }

    std::lock_guard<std::mutex> guard(focus_mutex, std::adopt_lock);

    ReqAstroAutoFocus req_start_astro_auto_focus;
    string serialized_data = ws_packet.data();
    if (!req_start_astro_auto_focus.ParseFromString(serialized_data)) {
        return -WS_PARSE_PROTOBUF_ERROR;
    }

    int mode = req_start_astro_auto_focus.mode();

    log_i("mode:%d", mode);

    if (cam_tele_.isCameraOpen() < 0) {
        wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, -CODE_CAMERA_TELE_CLOSED);
        return -CODE_CAMERA_TELE_CLOSED;
    }

    if (astro_fast_focus_state_ != STATE_IDLE || astro_slow_focus_state_ != STATE_IDLE || normal_focus_state_ != STATE_IDLE) {
        log_i("in auto focusing");
        wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);
        return 0;   
    }

    itips_observer_->updateCmdIdAndErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, -1);
    // opMode_t expmode;
    // double oldexp;
    // int oldgain,oldircut;
    // cam_tele_.getExpMode(&expmode);
    // if (expmode == OP_MANUAL) {
    //     cam_tele_.getExp(&oldexp);
    //     cam_tele_.getGain(&oldgain);
    //     cam_tele_.getIrCut(&oldircut);
    // }

    int ret = startAstroAutoFocusNew(mode);

    // if (expmode == OP_MANUAL) {
    //     cam_tele_.setExp(oldexp);
    //     cam_tele_.setGain(oldgain);
    //     cam_tele_.setIrCut(oldircut);
    // }

    return ret;

}

int Focus::getInfinityFocusPoint(float& infinity_focus_point_from_file) {
    if (fs::exists(astro_config_file_path_)) {
        std::ifstream astro_config_file(astro_config_file_path_);
        if (astro_config_file.is_open()) {
            nlohmann::json root;
            try {
                astro_config_file >> root;
            } catch (const std::exception& e) {
                log_e("read file exception: %s", e.what());
                astro_config_file.close();

                return -CODE_FOCUS_ASTRO_AUTO_FOCUS_FAST_ERROR;
            }

            astro_config_file.close();
            infinity_focus_point_from_file = root["infinity_focus_point"];

            return 0;
        } else {
            return -CODE_FOCUS_ASTRO_AUTO_FOCUS_FAST_ERROR;
        }
    } else {
        return -CODE_FOCUS_ASTRO_AUTO_FOCUS_FAST_ERROR;
    }
}

int Focus::startAstroAutoFocusNew(int mode) {
    b_start_astro_autofocus = true;

    if_first_star = true;
    hfa_data.clear();
    suitable_hfa_data.clear();

    int start_point = 50;

    int focus_step = 0;
    int focus_pulse = 2;
    int max_focus_step = 80 / focus_pulse;
    // int max_focus_step = 80;

    int consecutive_rise_total = 5;
    int consecutive_fall_total = 5;

    int consecutive_rise_count = 0;
    int consecutive_fall_count = 0;

    bool consecutive_rise_done = false;
    bool consecutive_fall_done = false;
    bool search_stop = false;

    // 直接移到无穷远位置
    if (mode == 1) {
        setAndNotifyFocusState(STATE_RUNNING, CAM_FUNCTION_ASTRO_AUTO_FOCUS_FAST);

        int has_sun_moon = checkWideSunMoon();
        //如果有太阳月亮，执行自动曝光
        if((has_sun_moon & 0x0000001) == 0x0000001 && autoExposure() == 0);
        //如果没有太阳月亮，但场景是明场的话，不改曝光
        else if((has_sun_moon & 0x0000010) == 0x0000010);
        //没有太阳月亮，场景也是暗场，则走星星曝光
        else{
            cam_tele_.setExp(1);
            cam_tele_.setGain(80);
            cam_tele_.setIrCut(1);
        }

#if TEST_ASTRO_AUTOFOCUS
        double exp;
        int gain;
        cam_tele_.getExp(&exp);
        cam_tele_.getGain(&gain);

        std::string file_path = album_path + astro_path + getTimeStamp() + "_" + std::to_string(exp) + "_" + std::to_string(gain) + "/";
        log_i("getAstroTestData file path is %s", file_path.c_str());
        fs::create_directory(file_path);
#endif
        float infinity_focus_point_from_file = 0;
        int ret = getInfinityFocusPoint(infinity_focus_point_from_file);
        if (ret < 0) {
            itips_observer_->updateCmdIdAndErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, -ret);
            wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, ret);
            setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_ASTRO_AUTO_FOCUS_FAST);
            setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS_FAST);

            return ret;
        }

        int start_position;
        motor_focus.resetInPulse(FOCUS_FAR, start_position);

        motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, FOCUS_NEAR, 305, infinity_focus_point_from_file, 0);
        motor_focus.waitMotorToStop(1);

        itips_observer_->updateCmdIdAndErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, WS_OK);
        wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_ASTRO_AUTO_FOCUS_FAST);
        setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS_FAST);

        return 0;

        // int start_point_from_file = infinity_focus_point_from_file - (consecutive_fall_total + 1) * focus_pulse;
        // start_point = start_point_from_file < 0 ? start_point : start_point_from_file;

        // max_focus_step = 40 / focus_pulse;
    }

    setAndNotifyFocusState(STATE_RUNNING, CAM_FUNCTION_ASTRO_AUTO_FOCUS);

    int has_sun_moon = checkWideSunMoon();
    //如果有太阳月亮，执行自动曝光，再执行自动对焦；或者如果没有太阳月亮，但场景是明场的话，执行普通的对焦，但不算对焦天文
    if(((has_sun_moon & 0x0000001) == 0x0000001 && autoExposure() == 0) || (has_sun_moon & 0x0000010) == 0x0000010) {
        int ret = autoFocus(0, 0, 0);

        cv::Rect match_rect;
        double match_value;
        if (ret == 0 && mode == 0) {
            std::tie(match_rect, match_value) = matchTemplate();
            log_i("matched x = %d, y = %d, width = %d, height = %d, value = %f", match_rect.x, match_rect.y, match_rect.width, match_rect.height, match_value);
            if (match_rect.x < 0) {}
            else {
                double diff_ratio_w = fabs((match_rect.x - default_template_rect.left_top_w) * 1.0 / default_template_rect.left_top_w) * 100;
                double diff_ratio_h = fabs((match_rect.y - default_template_rect.left_top_h) * 1.0 / default_template_rect.left_top_h) * 100;
                log_i("diff_ratio_w = %.f, diff_ratio_h = %.f", diff_ratio_w, diff_ratio_h);
                if (diff_ratio_w < 10.0 && diff_ratio_h < 10) {
                    cur_template_rect.left_top_w = match_rect.x;
                    cur_template_rect.left_top_h = match_rect.y;
                    cur_template_rect.width = match_rect.width;
                    cur_template_rect.height = match_rect.height;
                    log_i("cur_template_rect left_top_w = %d, left_top_h = %d", match_rect.x, match_rect.y);
                    wsResNotifyPictureMatching(CMD_NOTIFY_TELE_WIDI_PICTURE_MATCHING, MODULE_FOCUS, CMD_TYPE_NOTIFICATION, ret, match_rect.x, match_rect.y, match_rect.width, match_rect.height, match_value);
                }
            }
        }

        wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, ret);
        setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
        setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS);

        return ret;
    } else {  // 没有太阳月亮，场景也是暗场，则走天文自动对焦
        cam_tele_.setExp(1);
        cam_tele_.setGain(80);
        cam_tele_.setIrCut(1);
    }

    int start_position;
    motor_focus.resetInPulse(FOCUS_FAR, start_position);

    motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, FOCUS_NEAR, 305, start_point, 0);
    motor_focus.waitMotorToStop(1);

    std::vector<float> hfa_index;
    std::vector<float> hfa_value;

    std::future<int> get_hfa_thread_;

    while (b_start_astro_autofocus) {
        MEDIA_BUFFER dst_mb = cam_tele_.getRgbMB_4k(1920, 1080, true);
        // rkfree(dst_mb);
        // dst_mb = cam_tele_.getRgbMB_4k(1920, 1080, true);

        if (dst_mb == NULL) {
			itips_observer_->updateCmdIdAndErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, CODE_CAMERA_TELE_GET_IMAGE_FAILED);
            wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, -CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR);
            setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
            setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS);

            return 0;
        }

        if (get_hfa_thread_.valid()) {
            int hfa_ret = get_hfa_thread_.get();
            if (hfa_ret == -2) {// 星星移动到边界，对焦失败
                rkfree(dst_mb);
				itips_observer_->updateCmdIdAndErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, CODE_FOCUS_ASTRO_STAR_CLOSE_TO_BORDER);
                wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, -CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR);
                setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
                setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS);

                return 0;
            }
        }

        // if (search_stop) {
        //     rkfree(dst_mb);
        //     break;
        // }

        get_hfa_thread_ = std::async(std::launch::async, [&, dst_mb, focus_step]() {
            Mat img_src = Mat(1080, 1920, CV_8UC3, (unsigned char*)RK_MPI_MB_GetPtr(dst_mb));
            
            float hfa_equivalent_radiu = 0.0;
            int ret = getHfd(img_src, hfa_equivalent_radiu);

#if TEST_ASTRO_AUTOFOCUS
            timer.start();

            std::string img_path = file_path + std::to_string(focus_step) + ".jpg";
            log_i("img path = %s", img_path.c_str());
            cv::cvtColor(img_src, img_src, cv::COLOR_RGB2BGR);
            imwrite(img_path, img_src);

            timer.stop();
            timer.show_distance("store img cost time");
#endif

            RK_MPI_MB_ReleaseBuffer(dst_mb);

            if (ret == 0) 
                hfa_data.emplace_back(focus_step, hfa_equivalent_radiu);

            return ret;

            // if (ret == 0) {
            //     hfa_data.emplace_back(focus_step, hfa_equivalent_radiu);

            //     if (hfa_data.size() >= 2) {
            //         if (consecutive_fall_done == false) {
            //             if (hfa_data[hfa_data.size() - 1] < hfa_data[hfa_data.size() - 2])
            //                 consecutive_fall_count++;
            //             else 
            //                 consecutive_fall_count = 0;
            //         } else {
            //             if (consecutive_rise_done == false) {
            //                 if (hfa_data[hfa_data.size() - 1] > hfa_data[hfa_data.size() - 2])
            //                     consecutive_rise_count++;
            //                 else
            //                     consecutive_rise_count = 0;
            //             }
            //         }

            //         if (consecutive_fall_count >= consecutive_fall_total) 
            //             consecutive_fall_done = true;

            //         if (consecutive_rise_count >= consecutive_rise_total) {
            //             consecutive_rise_done = true;
            //             search_stop = true;
            //         }
            //     }
            // } else {
            //     consecutive_rise_count = 0;
            //     consecutive_fall_count = 0;
            //     consecutive_rise_done = false;
            //     consecutive_fall_done = false;
            // }
            
        });

        if (focus_step >= max_focus_step)
            break;

        motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, FOCUS_NEAR, 305, focus_pulse, 0);
        motor_focus.waitMotorToStop(1);

        focus_step++;
    }

    if (get_hfa_thread_.valid()) 
        get_hfa_thread_.wait();

    if (b_start_astro_autofocus == false) {
        if (itips_observer_->getErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS) == -1)
            itips_observer_->backToOldCmdIdAndErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS);

        wsCommonResponse(CMD_FOCUS_STOP_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);
        setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
        setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
        
        return 0;
    }

    suitable_hfa_data.push_back(hfa_data);

    int maxLength = 0;
    std::vector<HfaData> longest_suitable_filedata;
    for (const std::vector<HfaData>& filedata : suitable_hfa_data) {
        if (filedata.size() > maxLength) {
            maxLength = filedata.size();
            longest_suitable_filedata = filedata;
        }
    }

    for (const HfaData& entry : longest_suitable_filedata) {
        // std::cout << "hfa_index " << entry.hfa_index << " hfa_value " << entry.hfa_value << std::endl;
        hfa_index.push_back(float(start_point + (entry.hfa_index * focus_pulse)));
        hfa_value.push_back(entry.hfa_value * entry.hfa_value);
    }

    if (longest_suitable_filedata.size() <= 10) {
		itips_observer_->updateCmdIdAndErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, CODE_FOCUS_ASTRO_DATA_NOT_ENOUGH);
        wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, -CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR);
        setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
        setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS);

        return -CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR;
    }

    // 选择最低点附近的点用于拟合
    std::vector<float>::iterator min_element_iterator = std::min_element(hfa_value.begin(), hfa_value.end());

    float min_value = *min_element_iterator;

    int min_index = std::distance(hfa_value.begin(), min_element_iterator);
    std::cout << "min_index " << min_index << std::endl;
    
    // 取最小值前最大值点作为拟合起点
    std::vector<float>::iterator max_element_before_min_element_iterator = std::max_element(hfa_value.begin(), min_element_iterator);
    int max_index_before_min_element_iterator = std::distance(hfa_value.begin(), max_element_before_min_element_iterator);
    // std::cout << "max_index_before_min_element_iterator " << max_index_before_min_element_iterator << std::endl;

    // 取最小值后最大值点作为拟合终点
    std::vector<float>::iterator max_element_after_min_element_iterator = std::max_element(min_element_iterator, hfa_value.end());
    int max_index_after_min_element_iterator = std::distance(hfa_value.begin(), max_element_after_min_element_iterator);
    // std::cout << "max_index_after_min_element_iterator " << max_index_after_min_element_iterator << std::endl;

    std::vector<float> hfa_index_crop(hfa_index.begin() + max_index_before_min_element_iterator, hfa_index.begin() + max_index_after_min_element_iterator + 1);
    std::vector<float> hfa_value_crop(hfa_value.begin() + max_index_before_min_element_iterator, hfa_value.begin() + max_index_after_min_element_iterator + 1);

    std::vector<float> hfa_crop_index(hfa_value_crop.size());
    for (int i = 0; i < hfa_value_crop.size(); i++) {
        // std::cout << "hfa_index " << hfa_index_crop[i] << " hfa_value " << hfa_value_crop[i] << std::endl;
        hfa_crop_index[i] = float(i);
    }

    std::vector<float> coeff_hfa_index_crop = polyfit(hfa_crop_index, hfa_index_crop, 1, std::vector<float>(), false);

    std::vector<float> coeff_hfa_value_crop = polyfit(hfa_crop_index, hfa_value_crop, 2, std::vector<float>(), false);

    std::vector<float> fit_min_point_hfa_value_crop(1, -coeff_hfa_value_crop[1] / (2 * coeff_hfa_value_crop[2]));
    std::vector<float> fit_min_point_hfa_index_crop = polyval(coeff_hfa_index_crop, fit_min_point_hfa_value_crop);
    std::vector<float> fit_min_value_hfa_value_crop = polyval(coeff_hfa_value_crop, fit_min_point_hfa_value_crop);

    std::vector<float> fit_left_max_point_hfa_value_crop(1, 0);
    std::vector<float> fit_left_max_value_hfa_value_crop = polyval(coeff_hfa_value_crop, fit_left_max_point_hfa_value_crop);

    std::vector<float> fit_right_max_point_hfa_value_crop(1, hfa_value_crop.size() - 1);
    std::vector<float> fit_right_max_value_hfa_value_crop = polyval(coeff_hfa_value_crop, fit_left_max_point_hfa_value_crop);

    int min_fit_index = fit_min_point_hfa_index_crop[0];
    std::cout << "min_fit_index " << min_fit_index << std::endl;

    int min_fit_value = fit_min_value_hfa_value_crop[0];
    // std::cout << "min_fit_value " << min_fit_value << std::endl;

    int left_max_fit_value = fit_left_max_value_hfa_value_crop[0];
    // std::cout << "left_max_fit_value " << left_max_fit_value << std::endl;

    int right_max_fit_value = fit_right_max_value_hfa_value_crop[0];
    // std::cout << "right_max_fit_value " << right_max_fit_value << std::endl;

    if (left_max_fit_value < min_fit_value || right_max_fit_value < min_fit_value) {
		itips_observer_->updateCmdIdAndErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, CODE_FOCUS_ASTRO_DATA_ERROR);
        wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, -CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR);
        setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
        setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS);

        return -CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR;
    }

    // 权衡拟合的最小值和实际最小值的差距
    int focus_point = min_fit_index;
    int compare_point = 0;
    const int error = 5;
    compare_point = (int)hfa_index[min_index];

    if (std::abs(focus_point - compare_point) > error) {
        std::cout << "polyfit is too bad, focus_point " << focus_point << std::endl;

		itips_observer_->updateCmdIdAndErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, CODE_FOCUS_ASTRO_DATA_ERROR);
        wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, -CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR);
        setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
        setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS);

        return -CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR;
    }

    motor_focus.setParameterInPositionInPulse(focus_point, 305, STEP_MOTOR_MINISTEP_1, STEP_MOTOR_RAMP_PULSE_0);
    motor_focus.run();

    std::ofstream astro_config_file(astro_config_file_path_);

    nlohmann::json astro_config = {
        {"infinity_focus_point", focus_point}
    };

    astro_config_file << std::setw(4) << astro_config << std::endl;
    astro_config_file.close();

#if TEST_ASTRO_AUTOFOCUS
    std::vector<float> test_value_hfa_value_crop = polyval(coeff_hfa_value_crop, hfa_crop_index);

    std::filesystem::path outputFilename = file_path / std::filesystem::path("D2_output.csv");
    std::filesystem::path outputFitFilename = file_path / std::filesystem::path("D2_output_fit.csv");

    // 打开文件以写入数据
    std::ofstream outputFile(outputFilename.string());
    std::ofstream outputFitFile(outputFitFilename.string());

    // 写入CSV格式的数据
    outputFile << "文件名,HFD" << std::endl;
    outputFitFile << "文件名,HFD" << std::endl;

    for (int i = 0; i < hfa_index_crop.size(); i++) {
        outputFile << std::to_string((int)hfa_index_crop[i]) + std::string(".jpg") << "," << std::pow(hfa_value_crop[i], 0.5) << std::endl;
        outputFitFile << std::to_string((int)hfa_index_crop[i]) + std::string(".jpg") << "," << std::pow(test_value_hfa_value_crop[i], 0.5) << std::endl;
    }

    outputFile.close();
    outputFitFile.close();

    std::cout << "数据已保存到" << outputFitFilename.string() << std::endl;
#endif

	itips_observer_->updateCmdIdAndErrorCode(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, WS_OK);
    wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);
    setAndNotifyFocusState(STATE_STOPPED, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
    setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
    
    return 0;
}

int Focus::stopAstroAutoFocusNew(WsPacket& ws_packet) {
    if (astro_slow_focus_state_ != STATE_IDLE && astro_slow_focus_state_ != STATE_STOPPING) {
        setAndNotifyFocusState(STATE_STOPPING, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
    } else if (astro_slow_focus_state_ == STATE_IDLE) {
        setAndNotifyFocusState(STATE_IDLE, CAM_FUNCTION_ASTRO_AUTO_FOCUS);
        wsCommonResponse(CMD_FOCUS_STOP_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);
    }

    b_start_astro_autofocus = false;

    return 0;
}

// 使用线程锁保护获取的状态和主动通知的状态之间的顺序，确保后面的状态不会先发送
void Focus::setAndNotifyFocusState(const StateNotify& state, int function_id) {
    std::unique_lock<std::mutex> lock(focus_state_mutex);

    if (function_id == CAM_FUNCTION_ASTRO_AUTO_FOCUS) {
        astro_slow_focus_state_ = state;

        log_i("astro_slow_focus_state_:%d", astro_slow_focus_state_.load());
    } else if (function_id == CAM_FUNCTION_ASTRO_AUTO_FOCUS_FAST) {
        astro_fast_focus_state_ = state;

        log_i("astro_fast_focus_state_:%d", astro_fast_focus_state_.load());
    } else if (function_id == CAM_FUNCTION_NORMAL_AUTO_FOCUS) {
        normal_focus_state_ = state;

        log_i("normal_focus_state_:%d", normal_focus_state_.load());
    } else if (function_id == CAM_FUNCTION_AREA_AUTO_FOCUS) {
        area_focus_state_ = state;
        log_i("area_focus_state_:%d", area_focus_state_.load());
    }

    wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, function_id, state);
}

void Focus::notifyFocusState() {
    std::unique_lock<std::mutex> lock(focus_state_mutex);

    if (astro_slow_focus_state_ != STATE_IDLE) {
        log_i("astro_slow_focus_state_:%d", astro_slow_focus_state_.load());
        wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_ASTRO_AUTO_FOCUS, astro_slow_focus_state_);
    } else if (astro_fast_focus_state_ != STATE_IDLE) {
        log_i("astro_fast_focus_state_:%d", astro_fast_focus_state_.load());
        wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_ASTRO_AUTO_FOCUS_FAST, astro_fast_focus_state_);
    } else if (normal_focus_state_ != STATE_IDLE) {
        log_i("normal_focus_state_:%d", normal_focus_state_.load());
        wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_NORMAL_AUTO_FOCUS, normal_focus_state_);
    } else if (area_focus_state_ != STATE_IDLE) {
        log_i("area_focus_state_:%d", area_focus_state_.load());
        wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_AREA_AUTO_FOCUS, area_focus_state_);
    } 
}

int Focus::startAstroAutoFocus(WsPacket& ws_packet) {
    log_i("start astro autofocus");
    int direction = 0;
    int index = 0;
    int pixPadding = 10;//像素补偿
    float pre_hfd = 0.0;
    float cur_hfd = 0.0;
    float min_hfd[5] = {100.0, 100.0, 100.0};
    int area_down_count = 0;
    int area_up_count = 0;
    int countinue_count = 3;
    bool b_continue_down = false;
    bool b_continue_up = false;
    int min_hfd_index = 0;
    int search_type = 0;//0-粗调 1-精调
    int last_step = 0;
    int last_index = 0;//最小hfd序号
    float hfd_area_thresholld = 2200.0;//hfd扫描过程中星点面积阈值，大于阈值停止扫描
    int last_scan_index = 30;//hfd扫描停止序号
    float pre_area = 0.0;
    float cur_area = 0.0;

    if (b_start_astro_autofocus) {
        log_i("in astro auto focusing");
        wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, WS_OK);
        return 0;   
    }
    
    b_start_astro_autofocus = true;

    // char timestamp[128];
    // get_timestamp(timestamp);
    // log_i("time stamp: %s", timestamp);
    // char cmd[256] = {0};
    // char find_star_img_path[256] = {0};
    // char reg_star_img_path[256] = {0};
    // snprintf(cmd, sizeof(cmd), "mkdir -p /userdata/Astro/autoFocus/findStar/%s", timestamp);
    // snprintf(find_star_img_path, sizeof(find_star_img_path), "/userdata/Astro/autoFocus/findStar/%s/", timestamp);
    // // system(cmd);
    // memset(cmd, 0, sizeof(cmd));
    // snprintf(cmd, sizeof(cmd), "mkdir -p /userdata/Astro/autoFocus/regStar/%s", timestamp);
    // snprintf(reg_star_img_path, sizeof(reg_star_img_path), "/userdata/Astro/autoFocus/regStar/%s/", timestamp);
    // // system(cmd);

    int start_position;
    motor_focus.resetInPulse(direction, start_position);

    direction = 1 - direction;
    motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, 50, 0);

    sleep(1);
    
    FrameT firstFrame;
    recognizerResultT recog_result;
   
    Rect rect;
    int ret = -1;
    int maxCount = 0;
    while(maxCount++ < 25 && ret < 0) {
        motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, 4, 0);
        // MEDIA_BUFFER dst_mb = cam_tele_.getRgbMB_4k(1920, 1080);
        MEDIA_BUFFER dst_mb = cam_tele_.getRgbMB_4k(3840, 2160);
        Mat img(2160, 3840, CV_8UC3, (unsigned char*)RK_MPI_MB_GetPtr(dst_mb));
        ret = findBrightestStar(img, rect, "");
        RK_MPI_MB_ReleaseBuffer(dst_mb);
    }

    if (ret < 0) {
        b_start_astro_autofocus = false;
        log_i("FindBrightestStar error, astro focus stop");
        wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR);
        wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_ASTRO_AUTO_FOCUS, STATE_STOPPED);
        return ret;
    }

    float test_x1 = rect.x;
    float test_y1 = rect.y;
    float test_x2 = rect.x + rect.width;
    float text_y2 = rect.y + rect.height;
    FrameT test_frame = std::make_tuple(test_x1, test_y1, test_x2, text_y2);
    recog_result.maxFrame = test_frame;
    log_i("index = %d, max_hfd = %f", index, recog_result.maxHfd);
    log_i("first recognizer frame : %f, %f, %f, %f", std::get<0>(recog_result.maxFrame), std::get<1>(recog_result.maxFrame), std::get<2>(recog_result.maxFrame), std::get<3>(recog_result.maxFrame));

    pre_hfd = 0.0;
    cur_hfd = 0.0;

    wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_ASTRO_AUTO_FOCUS, STATE_RUNNING);
    
    while (b_start_astro_autofocus) {
        index++;
        if (index > last_scan_index + 3)
            break;

        if (0 == search_type) {
            motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, 2, 0);
        } 

        if (1 == search_type) {
            if (last_scan_index + 1 == index) {
                motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, 1, 0);
                sleep(1);
            }

            if (last_scan_index + 2 == index) {
                motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, 2, 0);
                sleep(1);
            }
                
            if (last_scan_index + 3 == index && last_step != 0) {
                 log_i("run last_step");
                motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, last_step, 0);
                sleep(1);
            }
        }

        float x1 = std::get<0>(recog_result.maxFrame);
        float y1 = std::get<1>(recog_result.maxFrame);
        float x2 = std::get<2>(recog_result.maxFrame);
        float y2 = std::get<3>(recog_result.maxFrame);
        log_i("before recognizer frame : %f, %f, %f, %f",x1, y1, x2, y2);
        MEDIA_BUFFER dst_mb = cam_tele_.getRgbMB_4k(1920, 1080);
        recog_result = star_recognizer(dst_mb, index, STAR_IMAGE, recog_result.maxFrame,  "");
        RK_MPI_MB_ReleaseBuffer(dst_mb);

        if (recog_result.code == -RECOGNIZER_NO_STAR) {
            log_i("recognizered no star, stop");
            b_start_astro_autofocus = false;
            wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR);
            wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_ASTRO_AUTO_FOCUS, STATE_STOPPED);
            return -CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR;
        } else if (recog_result.code == -RECOGNIZER_IMAGE_ERROR) {
            log_i("recognizered image error, continue");
            continue;
        }

        log_i("index = %d, max_hfd = %f, max_w = %d", index, recog_result.maxHfd, recog_result.maxW);

        float new_width = std::get<2>(recog_result.maxFrame) - std::get<0>(recog_result.maxFrame);
        float new_height = std::get<3>(recog_result.maxFrame) - std::get<1>(recog_result.maxFrame);
        float new_x1 = new_x1 = x1 + std::get<0>(recog_result.maxFrame) - pixPadding;
        float new_y1 = y1 + std::get<1>(recog_result.maxFrame) - pixPadding;
        float new_x2 = new_x1 + new_width + pixPadding * 2;
        float new_y2 = new_y1 + new_height + pixPadding * 2;
        
        log_i("after recognizer frame : %f, %f, %f, %f", std::get<0>(recog_result.maxFrame), std::get<1>(recog_result.maxFrame), std::get<2>(recog_result.maxFrame), std::get<3>(recog_result.maxFrame));
        FrameT new_frame = std::make_tuple(new_x1, new_y1, new_x2, new_y2);
        recog_result.maxFrame = new_frame;
        log_i("update frame : %f, %f, %f, %f", std::get<0>(recog_result.maxFrame), std::get<1>(recog_result.maxFrame), std::get<2>(recog_result.maxFrame), std::get<3>(recog_result.maxFrame));
        pre_hfd = cur_hfd;
        cur_hfd = recog_result.maxHfd;
        
        log_i("pre_hfd = %f, cur_hfd = %f", pre_hfd, cur_hfd);

        if (0 == search_type && cur_hfd < min_hfd[1]) {
            min_hfd[1] = cur_hfd;
            min_hfd_index = index;
            log_i("min_hfd = %f, index = %d", min_hfd[1], index);
        }

        //先回到搜索到的最小hfd位置
        pre_area = cur_area;
        cur_area = pow(new_width / 2, 2) * 3.14159;
        log_i("pre_area = %f, cur_area = %f, index = %d", pre_area, cur_area, index);
        if (cur_area > pre_area) {
            area_up_count += 1;
        } else {
            area_up_count = 0;
        }

        if (area_up_count == countinue_count) {
            log_i("area_up_count = %d", area_up_count);
            last_scan_index = index;
        }
        
        if (cur_area > hfd_area_thresholld || 30 == index) {
            last_scan_index = index;
        }
        
        if (0 == search_type && last_scan_index == index) {
            direction = 1 - direction;
            motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, (last_scan_index - min_hfd_index) * 2, 0);
            sleep(1);
            search_type = 1;   
        } 

        if (1 == search_type) {
            if (last_scan_index + 1 == index) {
                direction = 1 - direction;
                min_hfd[0] = cur_hfd;
            }
            if (last_scan_index + 2 == index) {
                min_hfd[2] = cur_hfd;
                log_i("min_hfd[0] = %f, min_hfd[1] = %f, min_hfd[2] = %f", 
                min_hfd[0], min_hfd[1], min_hfd[2]);
                float last_min_hfd = 100.0;
                
                for (int i = 0; i < 3; i++) {
                    if (min_hfd[i] < last_min_hfd) {
                        last_min_hfd = min_hfd[i];
                        last_index = i;
                    }
                }

                if (last_index - 1 < 0) {
                    direction = 1 - direction;
                }
                last_step = abs(2 - last_index);
                log_i("get last_min_hfd = %f, last_step = %d", last_min_hfd, last_step);
            }
        }
    }
    
    b_start_astro_autofocus = false;

    wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_ASTRO_AUTO_FOCUS, STATE_STOPPED);
    wsCommonResponse(CMD_FOCUS_START_ASTRO_AUTO_FOCUS, MODULE_FOCUS, CMD_TYPE_RESPONSE, CODE_FOCUS_ASTRO_AUTO_FOCUS_SLOW_ERROR);
    
    return 0;

}


int Focus::getAstroTestData(WsPacket& ws_packet) {
    int direction = 0;
    double exp;
    int gain;
    cam_tele_.getExp(&exp);
    cam_tele_.getGain(&gain);

    std::string file_path = album_path + astro_path + getTimeStamp() + "_" + std::to_string(exp) + "_" + std::to_string(gain) + "/";
    log_i("getAstroTestData file path is %s", file_path.c_str());
    fs::create_directory(file_path);

    int start_position;
    motor_focus.resetInPulse(direction, start_position);

    direction = 1 - direction;
    motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, 50, 0);

    int count = 0;

    wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_ASTRO_AUTO_FOCUS, STATE_RUNNING);

    while (count++ < 80) {
        std::string img_path = file_path + std::to_string(count) + ".jpg";
        log_i("img path = %s", img_path.c_str());
        cam_tele_.photograph(img_path);

        motor_focus.driver.run(0, STEP_MOTOR_MINISTEP_1, direction, 305, 1, 0);
        usleep(500 * 1000);
        while (cam_tele_.getPhotoState() != CAM_TELE_PHOTO_IDLE) {
            usleep(100 * 1000);
        }
    }

    wsResNotifyCamFunctionState(CMD_NOTIFY_TELE_FUNCTION_STATE, MODULE_NOTIFY, CMD_TYPE_NOTIFICATION, CAM_FUNCTION_ASTRO_AUTO_FOCUS, STATE_STOPPED);

    return 0;
}